#pragma config(Sensor, in1,    PotArm,              sensorPotentiometer)
#pragma config(Sensor, in2,    DetectTube,          sensorLineFollower)
#pragma config(Sensor, dgtl1,  EncoderR,            sensorQuadEncoder)
#pragma config(Sensor, dgtl3,  EncoderL,            sensorQuadEncoder)
#pragma config(Sensor, dgtl5,  BaseSonar,           sensorSONAR_cm)
#pragma config(Sensor, dgtl11, ArmUp,               sensorTouch)
#pragma config(Sensor, dgtl12, ArmDown,             sensorTouch)
#pragma config(Motor,  port1,           DriveRB,       tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port2,           DriveRF,       tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port3,           DriveLF,       tmotorNormal, openLoop)
#pragma config(Motor,  port4,           ArmRL,         tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port5,           ArmLL,         tmotorNormal, openLoop)
#pragma config(Motor,  port6,           ArmRU,         tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port7,           ArmLU,         tmotorNormal, openLoop)
#pragma config(Motor,  port8,           SuckR,         tmotorNormal, openLoop)
#pragma config(Motor,  port9,           SuckL,         tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port10,          DriveLB,       tmotorNormal, openLoop, reversed)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#pragma platform(VEX)

//Competition Control and Duration Settings
#pragma competitionControl(Competition)
#pragma autonomousDuration(20)
#pragma userControlDuration(120)

#include "Vex_Competition_Includes.c"   //Main competition background code...do not modify!
#include "delta_lib_V2.c" //Main Funtion Library

void pre_auton()
{
  SensorValue[EncoderL] = 0;
  SensorValue[EncoderR] = 0;

  // All activities that occur before the competition starts
  // Example: clearing encoders, setting servo positions, ...
}

task autonomous()
{
  SensorValue[EncoderR] = 0;	  // Set the encoder so that it starts counting at 0
  SensorValue[EncoderL] = 0;	  // Set the encoder so that it starts counting at 0
  while(1 == 1)		// Creates an infinite loop, since "true" always evaluates to true
  {
    if (SensorValue[EncoderR] <500 &&  SensorValue[EncoderL] < 500){
      if(SensorValue[EncoderR] == SensorValue[EncoderL]) // If rightEncoder has counted the same amount as leftEncoder:
      {
        // Move Forward
        motor[DriveRB]  = 80;		    // Left Motor is run at power level 80
        motor[DriveRF]  = 80;		    // Left Motor is run at power level 80
        motor[DriveLB] = 100;		    // Right Motor is run at power level 80
        motor[DriveLF] = 100;		    // Right Motor is run at power level 80
        motor[SuckR] = -127;
        motor[SuckL] = -127;

      }
      else if(SensorValue[EncoderR] > SensorValue[EncoderR])	// If rightEncoder has counted more encoder counts
      {
        // Turn slightly right
        motor[DriveRB] = 60;		    // Left Motor is run at power level 60
        motor[DriveRF] = 60;		    // Left Motor is run at power level 60
        motor[DriveLB] = 100;		    // Right Motor is run at power level 80
        motor[DriveLF] = 100;		    // Right Motor is run at power level 80
        motor[SuckR] = -127;
        motor[SuckL] = -127;
      }
      else	// Only runs if leftEncoder has counted more encoder counts
      {
        // Turn slightly left

        motor[DriveRB] = 80;		    // Left Motor is run at power level 80
        motor[DriveRF] = 80;		    // Left Motor is run at power level 80
        motor[DriveLB] = 60;		    // Right Motor is run at power level 80
        motor[DriveLF] = 60;		    // Right Motor is run at power level 80
        motor[SuckR] = -127;
        motor[SuckL] = -127;
      }
    }
    else {
      motor[DriveRB] = 0;		    // Left Motor is run at power level 80
      motor[DriveRF] = 0;		    // Left Motor is run at power level 80
      motor[DriveLB] = 0;		    // Right Motor is run at power level 80
      motor[DriveLF] = 0;		    // Right Motor is run at power level 80
      motor[SuckR] = 0;
      motor[SuckL] = 0;
    }
  }

  ////////advanceed///////
  //int arm_in_position = 0;  //arm is down; 0 for false and 1 for true

  //drive_suck_linetube(100,FULL,5);//drive straight and inhale until red stack is possesed
  /*while(arm_in_position != 1) {
  arm_in_position = lock(low_lock);
  }

  turn(127,-90);//speed,degrees turn left

  while(arm_in_position != 1) {//place tubes
  arm_in_position = lock(low_descore);
  }

  drive(-FULL,5);// back up
  turn(FULL,180); //left or shouldn't matter, turn to face blue stack

  drive_suck(FULL,FULL,10);//drive to blue stack//drive straight and inhale the blue stack at the same time

  turn(FULL,90);//turn right
  drive(FULL,7);//drive straight
  turn(FULL,-45);//turn left to face tower
  drive(FULL,7);//drive to tower
  sucker(-FULL,3);//spit tube into tower
  */


}

task usercontrol()
{
  // User control code here, inside the loop

  while (true)
  {
    //Manual_Arm//
    if(vexRT[Ch3] < 15 && vexRT[Ch3] > -15){//Trim, if stick is between 15 & negative 15 motors equal 0.
        setArmSpeed(0);
    }
    else{//motors = stick angle
        setArmSpeed(vexRT[Ch3]);
      //goal_lock=0;
    }


    //INHALE//
    switch(vexRT[Btn5U] - vexRT[Btn5D])
    {
    case  1:motor[SuckR] = FULL;
      motor[SuckL] = FULL;
      break;
    case -1:motor[SuckR] = -FULL;
      motor[SuckL] = -FULL;
      break;
    case  0:motor[SuckR] = 0;
      motor[SuckL] = 0;
      break;
    }//: switch

    //Drive_Train//
    setDriveRSpeed((vexRT[Ch2] - vexRT[Ch1]));// (y + x)
    setDriveLSpeed((vexRT[Ch2] + vexRT[Ch1]));// (y - x)
  }
}
